#include "pid.h"

double pid_calculate(PID_STRUCTURE *_pid)
{
    // Calculate error
    double error = _pid->setpoint - _pid->pv;

    // Proportional term
    double Pout = _pid->_P * error;

    // Integral term
    _pid->_integral += error * _pid->_dt;
    double Iout = _pid->_I * _pid->_integral;

    // Derivative term
    double derivative = (error - _pid->_pre_error) / _pid->_dt;
    double Dout       = _pid->_D * derivative;

    // Calculate total output
    double output = Pout + Iout + Dout;

    //   Restrict to max/min
    if (output >= _pid->_max)
    {
        output = _pid->_max;
    }
    else if (output <= _pid->_min)
    {
        output = _pid->_min;
    }

    // Save error to previous error
    _pid->_pre_error = error;

    return output;
}

void setNextCalculateArguments(PID_STRUCTURE _pid, double setpoint, double pv)
{
    _pid.setpoint = setpoint;
    _pid.pv       = pv;
}
